/*
 * Servo output control
 *
 * by Zik Saleeba 2009-11-22
 */

#include "ffox.h"
#include "board.h"
#include "servo.h"

/* the number of timer peripherals we're using for servo output */
#define NUM_TIMER_UNITS 2

/* module variables */
static unsigned int ServoPos[SERVO_NUM_TIMESLOTS];

/* the list of timer peripherals we're using */
static TIM_TypeDef *ServoTimerUnit[NUM_TIMER_UNITS] = { TIM4, TIM8 };


/*
 * NAME:        Servo_Init
 * ACTION:      Initialise the servo subsystem. This sets up timer 2
 *              to do the timing.
 */
 
void Servo_Init( void )
{
    int Count;
    GPIO_InitTypeDef GPIO_InitStructure;
    TIM_OCInitTypeDef  TIM_OCInitStructure;
    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
   
    /* set up the initial positions for each channel */
    for (Count = 0; Count < SERVO_NUM_TIMESLOTS; Count++)
        ServoPos[Count] = SERVO_MID_US;
    
    /* TIM4 and TIM8 clock enable */
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);

    /* GPIO Configuration: configure servo I/O pins as alternate function push-pull */
    GPIO_StructInit(&GPIO_InitStructure);
    GPIO_InitStructure.GPIO_Pin = SERVO1_PIN | SERVO2_PIN | SERVO3_PIN | SERVO4_PIN;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(SERVO1_4_PORT, &GPIO_InitStructure);
    
    GPIO_InitStructure.GPIO_Pin = SERVO5_PIN | SERVO6_PIN | SERVO7_PIN | SERVO8_PIN;
    GPIO_Init(SERVO5_8_PORT, &GPIO_InitStructure);
    
    /* time base configuration for PWM servos - 1us resolution, 20ms period */
    TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
    TIM_TimeBaseStructure.TIM_Period = 19999;       /* period between pulses = 20ms */
    TIM_TimeBaseStructure.TIM_Prescaler = (PERIPHERAL_CLOCK / 1000000) - 1;  /* 1us resolution */
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    
    /* set all the timer outputs running in PWM mode */
    for (Count = 0; Count < NUM_TIMER_UNITS; Count++)
    {
        TIM_TypeDef *ThisTimer = ServoTimerUnit[Count];
        
        TIM_TimeBaseInit(ThisTimer, &TIM_TimeBaseStructure);

        /* PWM1 mode configuration */
        TIM_OCStructInit(&TIM_OCInitStructure);
        TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
        TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
        TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
        TIM_OCInitStructure.TIM_Pulse = SERVO_MID_US;

        TIM_OC1Init(ThisTimer, &TIM_OCInitStructure);
        TIM_OC1PreloadConfig(ThisTimer, TIM_OCPreload_Enable);

        TIM_OC2Init(ThisTimer, &TIM_OCInitStructure);
        TIM_OC2PreloadConfig(ThisTimer, TIM_OCPreload_Enable);

        TIM_OC3Init(ThisTimer, &TIM_OCInitStructure);
        TIM_OC3PreloadConfig(ThisTimer, TIM_OCPreload_Enable);

        TIM_OC4Init(ThisTimer, &TIM_OCInitStructure);
        TIM_OC4PreloadConfig(ThisTimer, TIM_OCPreload_Enable);
    
        TIM_ARRPreloadConfig(ThisTimer, ENABLE);
    
        /* enable counter */
        TIM_Cmd(ThisTimer, ENABLE);
    }
}


/*
 * NAME:        Servo_Set
 * ACTION:      Set the position of a servo
 * PARAMETERS:  int Port - which servo to move (0-7)
 *              int Position - what position to move to (in ms)
 */
 
void Servo_Set(int Port, int Position)
{
    /* check for a sensible port number */
    if (Port >= 0 && Port < NUM_SERVO_OUTPUTS)
    {
        /* clamp to a sensible value */
        if (Position < SERVO_SENSIBLE_POSITION_MIN)
            Position = SERVO_SENSIBLE_POSITION_MIN;
        
        if (Position > SERVO_SENSIBLE_POSITION_MAX)
            Position = SERVO_SENSIBLE_POSITION_MAX;
        
        /* set the position */
        ServoPos[Port] = Position;
        
        switch (Port)
        {
            case 0: TIM_SetCompare1(TIM4, Position); break;
            case 1: TIM_SetCompare2(TIM4, Position); break;
            case 2: TIM_SetCompare3(TIM4, Position); break;
            case 3: TIM_SetCompare4(TIM4, Position); break;
            case 4: TIM_SetCompare1(TIM8, Position); break;
            case 5: TIM_SetCompare2(TIM8, Position); break;
            case 6: TIM_SetCompare3(TIM8, Position); break;
            case 7: TIM_SetCompare4(TIM8, Position); break;
        }
    }
}


/*
 * NAME:        Servo_Get
 * ACTION:      Get the current position of a servo
 * PARAMETERS:  int Port - which servo to get (0-7)
 * RETURNS:     int - the current servo position (in ms)
 */
 
int Servo_Get(int Port)
{
    if (Port >= 0 && Port < NUM_SERVO_OUTPUTS)
        return ServoPos[Port];
    else
        return 0;
}
